#include "app.h"
#include "bsp/cpu_ticks.h"
#include "os/types.h"
#include "os/os.h"
#include "controller/motor.h"
#include "protocols/command.h"
#include "protocols/protocol.h"

static u32 app_task(void*);
static u32 plot_task(void*);

void app_start(void) {
	protocol_init();
	motor_init(motor(0));
    os_task_create(app_task, NULL);
	os_task_create(plot_task, NULL);
}


#define P_DATA(v) ((s16)((v) * 32.0f))
#define P_DUTY(v) ((s16)((v) * 32767.0F))
s16 app_get_plot_data(int id) {
	switch(id) {
		case PHASEA_CURR_IDX:
		case PHASEB_CURR_IDX:
		case PHASEC_CURR_IDX:
			return P_DATA(controller(motor(0))->phase_curr[id]);
		case PHASEA_VOL_IDX:
		case PHASEB_VOL_IDX:
		case PHASEC_VOL_IDX:
			return P_DATA(0);
		case SVPWM_DUTYA_IDX:
		case SVPWM_DUTYB_IDX:
		case SVPWM_DUTYC_IDX:
			return P_DUTY(controller(motor(0))->foc.duty_norm[id-SVPWM_DUTYA_IDX]);
		case INPUT_TORQUE_IDX:
			return P_DATA(controller(motor(0))->torque_target.interpolation);
		case OUTPUT_TORQUE_IDX:
			return P_DATA(controller(motor(0))->curr_vector_real);
		case INPUT_VELOCITY_IDX:
			return (controller(motor(0))->vel_target.interpolation);
		case ENCODER_VELOCITY_IDX:
			return (controller(motor(0))->encoder_velocity);
		case SENSORLESS_ANGLE_IDX:
			return P_DATA(0);
		case ENCODER_ANGLE_IDX:
			return P_DATA(rads_2_degree(controller(motor(0))->encoder_angle));
		case SENSORLESS_VELOCITY_IDX:
			return (0);
		case D_AXIS_CURR_IDX:
			return P_DATA(controller(motor(0))->foc.curr_dq_real.d);
		case Q_AXIS_CURR_IDX:
			return P_DATA(controller(motor(0))->foc.curr_dq_real.q);
		case D_TARGET_CURR_IDX:
			return P_DATA(controller(motor(0))->foc.curr_dq_set.d);
		case Q_TARGET_CURR_IDX:
			return P_DATA(controller(motor(0))->foc.curr_dq_set.q);
		case D_AXIS_VOL_IDX:
			return P_DATA(controller(motor(0))->foc.vol_dq_out.d);
		case Q_AXIS_VOL_IDX:
			return P_DATA(controller(motor(0))->foc.vol_dq_out.q);
		case DC_CURRENT_IDX:
			return P_DATA(controller(motor(0))->vbus_curr_sampled);
		default:
			return 0;
	}
}

void app_send_plot_datas(void) {
	u8 data[14];
	int i = 0;
	int offset = 0;
	for (; i < PLOT_MAX_IDS; i++) {
		u8 id = motor(0)->plot_ids[i];
		if (id == 0xFF) {
			break;
		}
		encode_u8(data+offset, id);
		offset += 1;
		encode_s16(data+offset, app_get_plot_data(id));
		offset += 2;
	}
	if (i == 0) {
		return;
	}
	protocol_send_indicator(CMD_Report_Plot, data, 2+i*3);
}

#ifdef USE_MEX_CMD
#define PLOT_TIME_US 1
#elif defined(CONFIG_BOARD_COM_CAN)
#define PLOT_TIME_US 10*1000
#else
#define PLOT_TIME_US 40
#endif
static u32 plot_task(void *ctx) {
	static u32 timestamp_us = 0;

	if (cpu_get_delta_us(timestamp_us) >= PLOT_TIME_US) {
		app_send_plot_datas();
		timestamp_us = cpu_get_usecond();
	}

	return 0;
}


static void report_motor_statistics(void) {
	mot_stat_t m;
	m.b_started = motor(0)->b_start;
	m.ctrl_mode = motor(0)->mode_running;
	m.sys_time = os_get_seconds();
	m.velocity = controller(motor(0))->encoder_velocity;
	m.vbus_curr = controller(motor(0))->vbus_curr_sampled * 32;
	m.vbus_vol = controller(motor(0))->vbus_vol_sampled * 32;
	m.curr_dq[0] = controller(motor(0))->foc.curr_dq_real.d * 32;
	m.curr_dq[1] = controller(motor(0))->foc.curr_dq_real.q * 32;
	m.vol_dq[0] = controller(motor(0))->foc.vol_dq_out.d * 32;
	m.vol_dq[1] = controller(motor(0))->foc.vol_dq_out.q * 32;
	protocol_send_indicator(CMD_Report_Stat, (u8 *)&m, sizeof(m));
}

static u32 app_task(void *p) {
	report_motor_statistics();
    return 300;
}


